#include "laser_controller.hpp"

/**
 * @brief Construct a new laser controller::laser controller object
 *
 * @param sock can to ethernet multicast socket
 * @param send_id can id, from laser device to this node
 * @param recv_id can id, from this node to laser device
 */
laser_controller::laser_controller(std::shared_ptr<iolayer::layer_socket> sock,
								   uint32_t send_id, uint32_t recv_id)
{
	this->sock = sock;
	this->_send_id = send_id;
	this->_recv_id = recv_id;
}

/**
 * @brief read a laser frame from network, skip unconcerned frames
 *
 * @param status laser status, 0 for idle, >0 while working
 * @param distance reading, 0.1 mm
 * @return int -1: socket read error(various reasons), -2: dlc incorrect, 0: can id mismatch,
 * >0: frame received, return dlc
 */
int laser_controller::read_frame(int &status, int &distance)
{
	uint32_t id;
	int dlc;
	uint8_t data[8];
	int err;
	err = this->sock->read(&id, &dlc, data);
	if (err <= 0)
	{
		return err;
	}
	if (id != this->_send_id)
	{
		return 0;
	}
	if (dlc != 8)
	{
		return -2;
	}

	status = data[0];
	distance = le32toh(*(uint32_t *)(data + 4));
	return dlc;
}

/**
 * @brief send one frame to enable or disable the laser
 * 
 * @param enable true: enable, false: disable
 * @return int >0: bytes written, -1: failed
 */
int laser_controller::write_frame(bool enable)
{
	uint8_t data[8];
	return this->sock->write(this->_recv_id, 8, data);
}
